package twoD.IsoPfem.Control;

public class SphereConstraintCondition implements IConstraintCondition {

	private double s_, s__;
	private double psi;
	private double lambda0;
	private int[] dofs_control;

	public SphereConstraintCondition(double s, double psi, int... dofs_control) {
		s_ = s;
		this.psi = psi;
		this.dofs_control = dofs_control;
	}

	@Override
	public double f(double[] u, double[] uold, double lambda, double lambda_old) {
		double tmp = 0;
		for (int i : dofs_control)
			tmp += ((u[i] - uold[i]) * (u[i] - uold[i]));
		tmp += (psi * psi * (lambda - lambda_old) * (lambda - lambda_old));
		tmp = Math.sqrt(tmp);
		return tmp - s_;
	}

	@Override
	public double[] dfdu(double[] u, double[] uold, double lambda,
			double lambda_old) {
		double[] tmp = new double[u.length];
		double norm_f = f(u, uold, lambda, lambda_old) + s_;
		for (int i : dofs_control)
			tmp[i] = (u[i] - uold[i]) / norm_f;
		return tmp;
	}

	@Override
	public double dfdlambda(double[] u, double[] uold, double lambda,
			double lambda_old) {
		double norm_f = f(u, uold, lambda, lambda_old) + s_;
		return psi * psi * (lambda - lambda_old) / norm_f;
	}

	@Override
	public void increment() {
		// do_nothing
	}

	@Override
	public void setInitialLambda(double lambda0) {
		this.lambda0 = lambda0;
	}

	@Override
	public double getInitialLambda() {
		return lambda0;
	}

	@Override
	public double getS() {
		return s_;
	}

	@Override
	public double[] getU(double[] uGlobal) {
		double[] u = new double[dofs_control.length];
		for (int i = 0; i < dofs_control.length; i++)
			u[i] = uGlobal[dofs_control[i]];
		return u;
	}

	@Override
	public void storeStepSize() {
		s__ = s_;
	}

	@Override
	public void restoreStepSize() {
		s_ = s__;
	}

	@Override
	public void reduceStepSize() {
		s_ /= 2;
	}

}
